# pragma once

# include "cpp_tools.h"
# include "udpSettings.h"
# include "dataStruct.h"
# include "tracking_base.h"

class smc_controller : public tracking_base
{
private:
    const double pi = cpp_tools::pi; // 圆周率

public:
    smc_controller()
        : tracking_base()
    {
        readReference();
    };

    ~smc_controller()
    {

    };

    std::pair<double,double> getControl(Eigen::VectorXd const & state, double delta, double F_x, double time) override
    {
        auto [linCurve,dynamic] = readStateVec(state);

        double s = linCurve.s;

        double v = dynamic.v;
        double beta = dynamic.beta;
        double omega = dynamic.omega;
        double e = linCurve.e;
        double Delta_phi = linCurve.Delta_phi;
        double Delta_varphi = Delta_phi - beta;

        double a_x = dynamic.a_x;
        double curve = curve_interp.interp(s);
        double v_ref = v_ref_interp.interp(s);
        double e_ref = e_ref_interp.interp(s);

        double mass = m_param.m_veh.m_body.mass_sprung + m_param.m_veh.m_body.mass_unsprung_front + m_param.m_veh.m_body.mass_unsprung_rear + 4*m_param.m_veh.m_tire.mass_wheel;
        double l_x = m_param.m_veh.m_body.l_f + m_param.m_veh.m_body.l_r;

        double F_x_des = 1*(v_ref - v)*mass;
        
        double Delta_phi_des = cpp_tools::saturation(2*(e_ref - e)/v,-0.78,0.78);
        double omega_ref = curve*v*std::cos(Delta_phi)/(1 - curve*e);
        double omega_des = 2*(Delta_phi_des - Delta_phi) + omega_ref;
        double delta_des = omega_des/(std::abs(omega_des) + 0.0001)*std::atan2(l_x,std::abs(v/omega_des));

        return {delta_des,F_x_des};
    };
};
